#include "pid.h"

_pid Position_pid;
_pid Increasement_pid;
/*******************************************************************************
  @brief         : void PID_param_init(float default_Kp,float default_Ki,float default_Kd,float deflaut_target,u8 posORinc)
  @param default_Kp：默认的Kp值（其余一致）
 @retval         : 无
 @note         : 用于初始化时，先给pid定初值
*******************************************************************************/
void PID_param_init(float default_Kp,float default_Ki,float default_Kd,float deflaut_target,u8 posORinc)
{
		/* 初始化参数 */
//    printf("PID_init begin \n");
    Position_pid.actual_val=0.0;
    Position_pid.err=0.0;
    Position_pid.err_last=0.0;
    Position_pid.err_last_last=0.0;
    Position_pid.integral=0.0;
	if(posORinc==is_PositionPID)
	{
		Position_pid.target_val=deflaut_target;				
		Position_pid.Kp = default_Kp;//0.01;
		Position_pid.Ki = default_Ki;//0.80;
		Position_pid.Kd = default_Kd;//0.04;
	}
	else 
	{
		Increasement_pid.target_val=deflaut_target;				
		Increasement_pid.Kp = default_Kp;//0.46
		Increasement_pid.Ki = default_Ki;//0.26;
		Increasement_pid.Kd = default_Kd;//0.00;
	}
}


/**
  * @brief  设置目标值
  * @param  val		目标值
	*	@note 	无
  * @retval 无
  */
void set_pid_target(float temp_val,u8 posORinc)
{
	if(posORinc==is_PositionPID)Position_pid.target_val = temp_val;
	else Increasement_pid.target_val = temp_val;    // 设置当前的目标值
}


/**
  * @brief  设置比例、积分、微分系数
  * @param  p：比例系数 P
  * @param  i：积分系数 i
  * @param  d：微分系数 d
  * @param  d：pid选项 posORinc
	*	@note 	无
  * @retval 无
  */
void set_p(float p,u8 posORinc)
{
    if(posORinc==is_PositionPID)Position_pid.Kp = p;
	else Increasement_pid.Kp = p;    // 设置比例系数 P
}
void set_i(float i,u8 posORinc)
{
	if(posORinc==is_PositionPID)Position_pid.Ki = i; 
	else Increasement_pid.Ki = i;   // 设置积分系数 I
}
void set_d(float d,u8 posORinc)
{
    if(posORinc==is_PositionPID)Position_pid.Kd = d;
	else Increasement_pid.Kd = d;    // 设置微分系数 D
}


/*******************************************************************************
 函 数 名         : Get_PID_Target_P_I_D(_pid* temp_pid,u8 posORinc)
 函数功能			 : 获取pid及target值。便于返回上位机进行分析
 输    入         : _pid* temp_pid 一个临时的_pid结构体变量的地址，用于取出参数值，posORinc选择是那种pid
 输    出         : 顺利读取则返回1
 说    明         : 无
*******************************************************************************/
u8 Get_PID_Target_P_I_D(_pid* temp_pid,u8 posORinc)
{
	if(posORinc==is_PositionPID)
	{
		temp_pid->target_val = Position_pid.target_val;
		temp_pid->Kp = Position_pid.Kp;
		temp_pid->Ki = Position_pid.Ki;
		temp_pid->Kd = Position_pid.Kd;
	}
	else
	{
		temp_pid->target_val = Increasement_pid.target_val;
		temp_pid->Kp = Increasement_pid.Kp;
		temp_pid->Ki = Increasement_pid.Ki;
		temp_pid->Kd = Increasement_pid.Kd;

	}
	return 1;
}



/**
  * @brief  位置式PID算法的实现
  * @param  val		实际值
  * @note 	无
  * @retval 通过PID计算后的输出
  */
float Position_PID_realize(float temp_val)
{
	/*计算目标值与实际值的误差*/
    Position_pid.err=Position_pid.target_val-temp_val;
	/*误差累积*/
    Position_pid.integral+=Position_pid.err;
	/*Position_PID算法实现*/
    Position_pid.actual_val=Position_pid.Kp*Position_pid.err+Position_pid.Ki*Position_pid.integral+Position_pid.Kd*(Position_pid.err-Position_pid.err_last);
	/*误差传递*/
    Position_pid.err_last=Position_pid.err;
	/*返回当前实际值*/
    return Position_pid.actual_val;
}


/**
  * @brief  Increasement_PID算法实现
  * @param  val		实际值
	*	@note 	无
  * @retval 通过Increasement_PID计算后的输出
  */
float Increasement_PID_realize(float temp_val) 
{
	/*计算目标值与实际值的误差*/
	Increasement_pid.err=Increasement_pid.target_val-temp_val;
	/*Increasement_PID算法实现*/
	if(Increasement_pid.err<-PID_Limition||Increasement_pid.err>PID_Limition)
	{
		float increment_val = Increasement_pid.Kp*(Increasement_pid.err - Increasement_pid.err_last) + Increasement_pid.Ki*Increasement_pid.err + Increasement_pid.Kd*(Increasement_pid.err - 2 * Increasement_pid.err_last + Increasement_pid.err_last_last);
		/*累加*/
		Increasement_pid.actual_val += increment_val;
	}
	/*传递误差*/
	Increasement_pid.err_last_last = Increasement_pid.err_last;
	Increasement_pid.err_last = Increasement_pid.err;
	/*返回当前实际值*/
	return Increasement_pid.actual_val;
}




